// no comment.

package org.lovelandhs.robotics.y2010;

import java.util.Vector;
import org.lovelandhs.robotics.y2010.input.InputManager;
import edu.wpi.first.wpilibj.*;
import org.lovelandhs.robotics.y2010.drive.Drive;
import org.lovelandhs.robotics.y2010.drive.Drive.Direction;
import org.lovelandhs.robotics.y2010.input.ButtonListener;
import org.lovelandhs.robotics.y2010.input.model.XBox360Model;

public class Robot extends IterativeRobot {

	private static Robot instance;
	private Hardware hardware;
	private InputManager inputManager;
	private Vector listeners;
	private long time1; // =System.currentTimeMillis();

	public Robot() {
		instance = this;

		Watchdog.getInstance().setEnabled(false);

		hardware = new Hardware();
		inputManager = new InputManager(new XBox360Model());

		getListeners();
	}

	public static Robot get() {
		return instance;
	}

	public Vector getListeners() {
		if (listeners == null) {
			listeners = new Vector();
			initListeners();
		}

		return listeners;
	}

	public void addListener(TurnListener l) {
		getListeners().addElement(l);
	}

	public void removeListener(TurnListener l) {
		getListeners().removeElement(l);
	}

	private void initListeners() {
		
	}

	public InputManager getInput() {
		return inputManager;
	}

	public void addButtonListener(String button, ButtonListener listener) {
		inputManager.getButton(button).addListener(listener);
	}

	public void removeButtonListener(String button, ButtonListener listener) {
		inputManager.getButton(button).removeListener(listener);
	}

	public Hardware getHardware() {
		return hardware;
	}

	public void robotInit() {
		new Thread(hardware.getCamera()).start();
		//Controller= new model.
	}

	public void autonomousInit() {
		time1 = System.currentTimeMillis();
	}

	public void autonomousPeriodic() {
		long time2 = System.currentTimeMillis();
		boolean zone1 = hardware.getSwitch1().get();
		boolean zone2 = hardware.getSwitch2().get();
		//System.out.println("Zone is"+Zone);
		//System.out.println("Zone_2 is"+Zone_2);

		Drive drive = hardware.getDrive();

		Direction direction = drive.getDirection();

		if (zone1 == false && zone2 == false) { //Zone 1
			while (time2 - time1 < 1000) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}
			while (time2 - time1 < 1100 && time2 - time1 > 1000) {
				drive.getFrontLeft().set(0 * direction.getModifier());
				drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());

				if (time2 - time1 < 1050 && time2 - time1 > 1000) {
					hardware.getKicker().push();
				}

				if (time2 - time1 < 1100 && time2 > 1050) {
					hardware.getKicker().retract();
				}
			}

			while (time2 - time1 < 1350 && time2 - time1 > 1100) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(-1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(-1 * direction.getModifier());
			}

			while (time2 - time1 < 2350 && time2 - time1 > 1250) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}

			while (time2 - time1 < 15000 && time2 - time1 > 2350) {
				drive.getFrontLeft().set(0 * direction.getModifier());
				drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());
			}
		}

		if (zone1 == true && zone2 == false) { //Zone 2
			while (time2 - time1 < 1000 && time2 - time1 > 0) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}

			while (time2 - time1 < 1100 && time2 - time1 > 1000) {
				drive.getFrontLeft().set(0 * direction.getModifier());
				drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());
				if (time2 - time1 < 1050 && time2 - time1 > 1000) {
					hardware.getKicker().push();
				}
				if (time2 - time1 < 1100 && time2 - time1 > 1050) {
					hardware.getKicker().retract();
				}
			}

			while (time2 - time1 < 2100 && time2 - time1 > 1100) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}

			while (time2 - time1 < 2200 && time2 - time1 > 2100) {
				drive.getFrontLeft().set(0 * direction.getModifier());
				drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());

				if (time2 - time1 < 2150 && time2 - time1 > 2000) {
					hardware.getKicker().push();
				}

				if (time2 - time1 < 2200 && time2 - time1 > 2150) {
					hardware.getKicker().retract();
				}
			}

			while (time2 - time1 < 2450 && time2 - time1 > 2200) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(-1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(-1 * direction.getModifier());
			}

			while (time2 - time1 < 3450 && time2 - time1 > 2450) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}

			while (time2 - time1 < 15000 && time2 - time1 > 3450) {
				drive.getFrontLeft().set(0 * direction.getModifier());
				drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());
			}
		}

		if (!zone1 && zone2) {
			while (time2 - time1 < 1000 && time2 - time1 > 0) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}

			while (time2 - time1 < 1100 && time2 - time1 > 1000) {
				drive.getFrontLeft().set(0 * direction.getModifier());
				drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());

				if (time2 - time1 < 1050 && time2 - time1 > 1000) {
					hardware.getKicker().push();
				}

				if (time2 - time1 < 1100 && time2 - time1 > 1050) {
					hardware.getKicker().retract();
				}
			}

			while (time2 - time1 < 2100 && time2 - time1 > 1100) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}

			while (time2 - time1 < 2200 && time2 - time1 > 2100) {
				drive.getFrontLeft().set(0 * direction.getModifier());
				drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());

				if (time2 - time1 < 2150 && time2 - time1 > 2000) {
					hardware.getKicker().push();
				}

				if (time2 - time1 < 2200 && time2 - time1 > 2150) {
					hardware.getKicker().retract();
				}
			}

			for (; time2 - time1 < 3200 && time2 - time1 > 2200;) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}
			for (; time2 - time1 < 3300 && time2 - time1 > 3200;) {
				drive.getFrontLeft().set(0 * direction.getModifier());
				drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());
				if (time2 - time1 < 3250 && time2 - time1 > 3200) {
					hardware.getKicker().push();
				}
				if (time2 - time1 < 3300 && time2 - time1 > 3250) {
					hardware.getKicker().retract();
				}
			}
			for (; time2 - time1 < 3550 && time2 - time1 > 3300;) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(-1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(-1 * direction.getModifier());
			}
			for (; time2 - time1 < 4550 && time2 - time1 > 3550;) {
				drive.getFrontLeft().set(1 * direction.getModifier());
				drive.getFrontRight().set(1 * direction.getModifier());
				drive.getBackLeft().set(1 * direction.getModifier());
				drive.getBackRight().set(1 * direction.getModifier());
			}
			for (; time2 - time1 < 15000 && time2 - time1 > 4550;) {
				drive.getFrontLeft().set(0 * direction.getModifier());
		 		drive.getFrontRight().set(0 * direction.getModifier());
				drive.getBackLeft().set(0 * direction.getModifier());
				drive.getBackRight().set(0 * direction.getModifier());
			}
		}

		for (int i = 0; i < listeners.size(); i++) {
			((TurnListener) listeners.elementAt(i)).onAutonomousTurn(this);
		}
	}

	public void teleopPeriodic() {
		hardware.getCamera().unpause();
		inputManager.processInput();

		for (int i = 0; i < listeners.size(); i++) {
			((TurnListener) listeners.elementAt(i)).onTeleopTurn(this);
		}

		// hardware.getDrive().tankDrive(hardware.getLeftStick(),
		// 	hardware.getRightStick());
		
	}

	public void disabledInit() {
		super.disabledInit();

		hardware.getCamera().pause();
	}

}
